/**
 * ros节点入口源文件
 * @author：胡云浩
 * */
#include "rearwheel_feedback.h"
#include <iostream>
#include <fstream>
#include <ros/ros.h>
#include "ros_rearwheel_feedback/rearwheel_feedback.h"  //ros msg header

using namespace std;
// 定义全局变量来发送和订阅消息
ros_rearwheel_feedback::rearwheel_feedback rwFeedback_msg_rx;  //接收消息的变量
ros_rearwheel_feedback::rearwheel_feedback rwFeedback_msg_tx;  //发送消息的变量

// 定义一个回调函数,来接受来自PreScan节点的ros msg.
void chatterCallBack(const ros_rearwheel_feedback::rearwheel_feedbackConstPtr &msg) {
    ROS_INFO("I receive [x, y, yaw, v] = [%f, %f, %f, %f]", msg->x, msg->y, msg->yaw, msg->v);
    rwFeedback_msg_rx = *msg;
}

void printVector(vector<float> &v1) {
    for (auto iter = v1.begin(); iter < v1.end(); iter++) {
        cout << *iter << endl;
    }
}

void rosRun(int argc, char **argv) {
    // 1.初始化ROS节点，并且给节点取一个名字
    ros::init(argc, argv, "n_rearwheel_feedback");
    // 2.创建一个handle,并且初始化它
    ros::NodeHandle n;
    // 3.节点n_rearwheel_feedback将要发送的topic
    ros::Publisher publisher = n.advertise<ros_rearwheel_feedback::rearwheel_feedback>("rearWheelControl", 1000);
    // 4.节点n_rearwheel_feedback将要接受的topic
    ros::Subscriber subscriber = n.subscribe("preScan", 1000, chatterCallBack);
    // 5.设置publisher的发布频率
    ros::Rate loop_rate(100);   //100HZ
    /*******************************6初始化控制器参数**********************************/
    // 6.1初始化车辆的状态
    controlNS::State state(-32, 35, myNumpy::deg2rad(200), 0);
    // 6.2计算路径相关的数据
    controlNS::CalcPath path(controlNS::control_json.at("path_x"), controlNS::control_json.at("path_y"),
                             controlNS::control_json.at("path_yaw"), state);
    // 7进入控制的循环
    while (ros::ok()){
        ros::spinOnce();    //ros 调用一次消息回调函数，用以接收来自PreScan的车辆姿态信息
        controlNS::RearWheelFeedbackControl rearWheelFeedbackControl;   //创建一个后轮反馈算法控制器
        float delta =  rearWheelFeedbackControl.calDelta(state, path); //给控制器传入车辆的状态和路径信息，用以计算方向盘转角
        // 传输方向盘转角和横向位置偏差给PreScan
        rwFeedback_msg_tx.error_rear_axle = path.cross_track_error_;
        rwFeedback_msg_tx.delta = myNumpy::rad2deg(delta) * controlNS::kSteeringRatio; //方向盘转角 = 车轮转角 * 传动比
        ROS_INFO("I send delta[%f]", rwFeedback_msg_tx.delta);
        //测试打印输出
//        cout << "diff1 = " << path.diff1_ << endl;
//        cout << "diff2 = " << path.diff2_ << endl;
//        cout << "curvature = " << path.curvature_ << endl;
        publisher.publish(rwFeedback_msg_tx);
        state.updateFromPreScan(rwFeedback_msg_rx.x, rwFeedback_msg_rx.y, rwFeedback_msg_rx.yaw, rwFeedback_msg_rx.v);  //用PreScan发过来的车辆状态信息更新本车状态信息
        path.calUpdate(state);
        loop_rate.sleep();  //让ros在剩余的时间睡眠，以达到设定的publisher的发布频率
    }
}


int main(int argc, char **argv) {
    rosRun(argc, argv);
}